#include "prm_2d.h"

#include <CGAL/squared_distance_2.h>
#include "CollisionDetector2D.h"
#include "Sampler2D.h"

const double EPS = 0.1;

namespace {
  double pointDistance(const Point_2& a, const Point_2& b) {
    return sqrt(CGAL::to_double(CGAL::squared_distance(a, b)));
  }
}  // namespace

Prm2D::Prm2D(const CollisionDetector2D& col, const Sampler2D& sampler)
    : m_collisionDetector(&col), m_sampler(&sampler), m_graph(0, true) { }

void Prm2D::generateRoadmap(Point_2 start, Point_2 target,
                             int numberVertices, int kNearest) {
  m_graph.add_vertex(start);
  m_graph.add_vertex(target);
  for (size_t i = 0; i < numberVertices; ++i) {
    m_graph.add_vertex(m_sampler->generate_sample());
  }
  vector<Point_2> vertices = m_graph.get_vertices();
  m_kdTree.insert(vertices.begin(), vertices.end());
  for (size_t i = 0; i < vertices.size(); ++i) {
    vector<Point_2> nn;
    m_kdTree.k_nearest_neighbors(vertices[i], kNearest+1, std::back_inserter(nn));
    for (vector<Point_2>::const_iterator iter = nn.begin(); iter != nn.end(); ++iter) {
      if (m_collisionDetector->local_planner(vertices[i], *iter, EPS)) {
        m_graph.add_edge(vertices[i], *iter, pointDistance(vertices[i], *iter));
      }
    }
  }
}

const Graph<Point_2>& Prm2D::getGraph() const {
  return m_graph;
}
